//
// SoftServTest --- test out the 'softserv' serial servo controller
//
// 2008, Tod E. Kurt, http://todbot.com/blog/
//

import processing.serial.*;

// Change this to the portname your Arduino board
String portname = "/dev/tty.usbserial-FTD5KY90";  //"/dev/tty.usbserial-A3000Xv0"; // or "COM5"

Serial port;
byte servoX;
byte servoY;

byte servoBX = 1;
byte servoBY = 0;
byte servoLX = 2;
byte servoLY = 3;
byte servoRX = 4;
byte servoRY = 5;

byte rate_fast = 20;
byte rate_slow = 2;
byte rate_current = rate_slow;

boolean moving = false;

void setup() {
  size(254,254);
  frameRate(20);
  smooth();
  background(40,40,40);
  noStroke();
  if( portname==null || portname.equals("") ) {
    portname = Serial.list()[0];
  }
  println("Opening serial port "+portname);
  port = new Serial(this, portname, 9600);
  port.write("i"); // set input mode to MiniSSC style
  softservSetRate( rate_current );
  servoX = servoBX;
  servoY = servoBY;

  println("hit space bar to toggle moving");
}

void draw() {
  if( moving ) {
    softservMoveServo( servoX, (byte)mouseX );
    softservMoveServo( servoY, (byte)mouseY );
  }
}

void keyPressed() {
  println("keypressed!");
  if( key == CODED ) {
    if( keyCode == UP ) {
      println("controlling body base");
      servoX = servoBX;
      servoY = servoBY;
    } 
    else if( keyCode == DOWN ) {
      println("controlling body base");
      servoX = servoBX;
      servoY = servoBY;
    } 
    else if( keyCode == LEFT ) {
      println("controlling left side");
      servoX = servoLX;
      servoY = servoLY;
    } 
    else if( keyCode == RIGHT ) {
      println("controlling right side");
      servoX = servoRX;
      servoY = servoRY;
    }
  }
  else if( key == ' ' ) {
    moving = !moving;
    println("moving: "+moving);
    softservSetMoving( moving );
  }
  else if( key == 'c' ) { 
    println("centering all servos");
    for( byte i=0; i<8; i++ ) 
      softservMoveServo( i, (byte)0x80 );
  }
  else if( key == 'd' ) {
    println("dump:");
    port.write( 0xFF );    // MiniSSC command code
    port.write( 0xFD );    // dump status command
    port.write( 0x00 );    // unneeded, but all cmds in MiniSSC mode are 3-bytes
  }
  else if( key == 'r' ) {
    rate_current = (rate_current==rate_slow) ? rate_fast : rate_slow;
    println("changing slew rate to "+rate_current);
    softservSetRate( rate_current );
  }
}


// called whenever serial data arrives
void serialEvent(Serial p) {
  int c = port.read();
  print((char)c);  // just echo any characters received
}

//
void softservMoveServo( byte servonum, byte pos ) {
  port.write( 0xFF );      // MiniSSC command mode 
  port.write( servonum );  // servo to change 
  port.write( pos );       // set pos
}
//
void softservSetRate( byte rate ) {
  port.write( 0xFF );    // MiniSSC command mode 
  port.write( 0xFB );    // change rate command 
  port.write( rate );    // set rate
}
//
void softservSetMoving( boolean moving ) {
  port.write( 0xFF );    // MiniSSC command code
  port.write( 0xFC );    // moving toggle command
  port.write( (moving) ? 0x01:0x00 );
}

